#include <stdlib.h>
#include "LinearInterpolationMap.h"
#include "Arduino.h"

LinearInterpolationMap* newLinearInterpolationMap() {
	LinearInterpolationMap* map;
	map = (LinearInterpolationMap*) malloc(sizeof(LinearInterpolationMap));
	map->init();
	return map;
}

void LinearInterpolationMap::init() {
	numberOfPoints = 0;
}

void LinearInterpolationMap::addPoint(ValuePoint* valuePoint) {
	int newNumberOfPoints = numberOfPoints + 1;
	int valuePointSize = sizeof(ValuePoint);
	int newArraySize = newNumberOfPoints * valuePointSize;
	ValuePoint** temp = (ValuePoint**) malloc(newArraySize);

	for (int i = 0; i < numberOfPoints; i++) {
		ValuePoint* existing = valuePoints[i];
		temp[i] = existing;
	}

	temp[numberOfPoints] = valuePoint;

	if (numberOfPoints > 0) {
		free(valuePoints);
	}

	numberOfPoints++; 
	valuePoints = temp;
}

int LinearInterpolationMap::mapInputValue(int inputValue) {
	if (numberOfPoints < 1) {
		return inputValue;
	}
	ValuePointRange range = getValuePointRangeForInputValue(inputValue);
	int minValue = range.lowerPoint->inputValue;
	int maxValue = range.upperPoint->inputValue;
	int minOutputValue = range.lowerPoint->outputValue;
	int maxOutputValue = range.upperPoint->outputValue;

	return map(inputValue, minValue, maxValue, minOutputValue, maxOutputValue);
}

ValuePointRange LinearInterpolationMap::getValuePointRangeForInputValue(int value) {
	ValuePointRange range;
	ValuePoint *lowerPoint = NULL;
	for (int i = 0; i < numberOfPoints; i++) {
		ValuePoint *point = valuePoints[i];
		if (value >= point->inputValue) {
			lowerPoint = point;
		} else {
			if (lowerPoint == NULL) {
				range.lowerPoint = point;
				range.upperPoint = point;
			} else {
				range.lowerPoint = lowerPoint;
				range.upperPoint = point;
			}
			return range;
		}
	}
	
	// the last two points
	int lowerIdx = numberOfPoints - 2;
	int upperIdx = numberOfPoints - 1;

	range.lowerPoint = valuePoints[lowerIdx];
	range.upperPoint = valuePoints[upperIdx];
	return range;
}

